package the_Crash;

import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.Set;

import repast.simphony.engine.schedule.ScheduleParameters;
import repast.simphony.engine.schedule.ScheduledMethod;
import repast.simphony.query.space.grid.GridCell;
import repast.simphony.query.space.grid.GridCellNgh;
import repast.simphony.random.RandomHelper;
import repast.simphony.space.SpatialMath;
import repast.simphony.space.continuous.ContinuousSpace;
import repast.simphony.space.continuous.NdPoint;
import repast.simphony.space.grid.Grid;
import repast.simphony.space.grid.GridPoint;
import repast.simphony.util.SimUtilities;

public class SmartCar extends Car {
	
	private static int numCars = 0;
	private int carId; //unique identifier, used to determine who is responsible for addressing the message between cars
	private GridPoint nextLocation;
	private Direction nextCurrentHeading;
	private Direction nextFutureHeading;
	private static int stepNumber = 0;
	private boolean didPlanMove = false;
	
	public SmartCar(SourceSink destination, SourceSink source, double maxSpeed,
			double accel, double deccel, GridPoint location, Grid<Object> grid,
			ContinuousSpace<Object> space) {
		super(destination, source, maxSpeed, accel, deccel, location, grid, space);
		numCars++;
		this.carId = numCars;
	}

	
	
	@ScheduledMethod (start = 1, interval = 1, priority = ScheduleParameters.FIRST_PRIORITY)
	public static void stepCounter(){
		stepNumber++;
	}
	
	//*******************************************************************************
	@Override
	@ScheduledMethod (start = 1, interval = 1)
	public void step()
	{
		//here we plan our next move and then make the move on alternating steps
		if(!done){
			ticksAtLight++;
			this.nextLight.watchCar(ticksAtLight, this.futureHeading, this.nextTurn, this.currentHeading);
			if(stepNumber%2 == 1 && !didPlanMove){
				changeSpeed();
				Message m = this.planMove();
				didPlanMove = true;
				if(m != null){
					this.sendMessage(m);
				}
			} else{
				this.makeMove();
				tickCount++;
			}
		}
	}
	
	//***********************************************************************************
	protected void makeMove(){
		if(this.nextLocation == null){
			this.done = true;
		}
		if(!done && CityBuilder.withinGrid(this.nextLocation.getX(), this.nextLocation.getY())){
			this.moveCarToLocation(this.nextLocation);
			this.currentHeading = this.nextCurrentHeading;
			this.futureHeading = this.nextFutureHeading;
		} else {
			CityBuilder.leftCity(this);
		}
		didPlanMove = false;
	}
	
	//**********************************************************************************
	protected Message planMove(){
		Direction initialHeading = this.currentHeading;
		Direction initialFutureHeading = this.futureHeading;
		GridPoint initialLocation = grid.getLocation(this);
		
		//this is going to actually move the car
		super.move();
		//get the car's new location
		this.nextLocation = grid.getLocation(this);
		
		if(nextLocation == null){
			this.done = true;
			return null;
		} else {
			//now we need to move the car back to its original location while it sends the message out to nearby cars of where it is going to move
			this.nextCurrentHeading = this.currentHeading;
			this.nextFutureHeading = this.futureHeading;
			this.moveCarToLocation(initialLocation);
			//reset to initial headings
			this.currentHeading = initialHeading;
			this.futureHeading = initialFutureHeading;
			return new Message(nextLocation, this);
		}
	}

	
	//*********************************************************************************

	@Override
	protected void receiveMessage(Message m) {
		//the car that has to adjust will be decided arbitrarily by whichever car has the lower id number
		if(nextLocation != null && m.getLocationToMoveTo()!=null && this.nextLocation.equals(m.getLocationToMoveTo())){
			if(this.carId > m.getSendingCar().carId && this!=m.getSendingCar() && this.speed > 0 && m.getSendingCar().speed > 0){
				//check if the car made a turn
				if(this.nextCurrentHeading.equals(this.futureHeading) && !this.futureHeading.equals(this.currentHeading)){//the car turned
					if(this.isCarBehindMe(m.getSendingCar())){
						//I will speed up slightly to move forward one unit to get out of the way
						this.gasIt();
						//send a message to the other car to slow down a little bit
						m.getSendingCar().easeUp();
					} else {
						//send a message to the other car to speed up slightly to move forward one unit to get out of the way
						m.getSendingCar().gasIt();
						this.easeUp();
					}
				} else if(this.didChangeLanes){
					if(this.isCarBehindMe(m.getSendingCar())){
						//send a message to the other car that they need to brake, and I will take the right of way
						m.getSendingCar().easeUp();
					} else {
						//I brake and allow the other car in
						this.easeUp();
					}
				} else{//we are just moving straight ahead normally
					if(this.isCarBehindMe(m.getSendingCar())){
						//send a message to the other car that they need to brake, and I will take the right of way by speeding up a little but
						m.getSendingCar().easeUp();
						this.gasIt();
					} else{
						//I brake and allow the other car in
						this.easeUp();
						m.getSendingCar().gasIt();
					}
				}
			}
			
		}
	}
	
	//********************************************************************************
	/*
	 * car will speed up slightly to get out of the way
	 */
	private void gasIt(){
		int[] velocity = this.getUnitVelocityFromHeading(this.nextCurrentHeading);
		this.nextLocation = new GridPoint(nextLocation.getX() + 2*velocity[0], nextLocation.getY() + 2*velocity[1]);
		this.sendMessage(new Message(nextLocation, this));
	}
	
	//***************************************************************************
	/*
	 * car will slow down to getout of the way of another car
	 */
	public void easeUp(){
		this.brake(1);
		this.sendMessage(this.planMove());
	}
	
	//********************************************************************************
	
	private void sendMessage(Message m){
		GridPoint currentLocation = grid.getLocation(this);
		if(currentLocation == null){
			return;
		}
		GridCellNgh <Car> nghCreator = new GridCellNgh<Car>(grid,currentLocation,Car.class ,ParameterResolver.getCommunicationDistance(),ParameterResolver.getCommunicationDistance());
		List <GridCell<Car>> gridCells = nghCreator.getNeighborhood (false);
		for(GridCell<Car> cell : gridCells){
			for(Car c : cell.items()){
				c.receiveMessage(m);
			}
		}
	}
	
	
	//*****************************************************************
	private void moveCarToLocation(GridPoint point){
		space.moveTo(this, point.getX(), point.getY());
		grid.moveTo(this, point.getX(), point.getY());
	}
	
	//******************************************************************************
	
	private boolean isCarBehindMe(Car c){
		GridPoint myLocation = grid.getLocation(this);
		GridPoint theirLocation = grid.getLocation(c);
		if(this.currentHeading.equals(Direction.North)){
			return theirLocation.getY() <= myLocation.getY();
		} else if(this.currentHeading.equals(Direction.South)){
			return theirLocation.getY() >= myLocation.getY();
		}  else if(this.currentHeading.equals(Direction.East)){
			return theirLocation.getX() <= myLocation.getX();
		}  else{
			return theirLocation.getX() >= myLocation.getX();
		}
	}
	
	

}
